#ifndef OBJECTLIST
#define OBJECTLIST
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <cmath>
#include <iostream>
#include <string>
#include <group6/DetectedObject.h>
#include <group6/DetectedObjectArray.h>
#include <geometry_msgs/Vector3.h>
#define Pii 3.1415926536


class ObjectList
{
public:
    ObjectList(ros::NodeHandle& nh, std::string publish_topic);
    ~ObjectList(){}

    void updateObjectList(const group6::DetectedObjectArray::ConstPtr& detectedObjects);

    void deleteObjects(const geometry_msgs::Vector3::ConstPtr& deletePoint);

    void publishObjects(bool publish_empty);

    bool distanceCheck(group6::DetectedObject newObject);

    bool shouldBeSeen(group6::DetectedObject newObject);

    bool notForbidden(group6::DetectedObject newObject);

    void addForbidden(double x, double y, double r);

    void printObjectList(int detectedObstaclesAmount);

    void setFOV(std::pair<double, double> xy1, std::pair<double, double> xy2,
                std::pair<double, double> xy3, std::pair<double, double> xy4);

    std::pair<double,double> solveXline(std::pair<double,double> xy1, std::pair<double,double> xy2);
    std::pair<double,double> solveYline(std::pair<double,double> xy1, std::pair<double,double> xy2);

private:
    //Min distance between objects.
    float min_distance;
    float distance;

    //Variables for object deleting.
    float del_distance;
    int deleted_objects;

    // ROS handles
    ros::NodeHandle mNode;
    ros::Publisher list_publisher;

    //Publish topic.
    std::string pub_topic;
    //Object lists/messages.
    group6::DetectedObjectArray objectArray;
    group6::DetectedObjectArray newObjectArray;
    group6::DetectedObject object;

    //Forbidden zones, x,y,r.
    group6::DetectedObjectArray forbidden_zones;

    //Timestamp for current time.
    ros::Time time_stamp;
   
    //Robot pose listeners.
    tf::TransformListener mListener;
    tf::StampedTransform currentTransform;
    double x_r;
    double y_r;
    double a_r;

    //Object in robot coordinates.
    double x_or;
    double y_or;

    //Line variables.
    std::pair<double,double> line;

    //Object limits.
    std::pair<double, double> x_min;
    std::pair<double, double> x_max;
    std::pair<double, double> y_min;
    std::pair<double, double> y_max;

    //Time variables.
    time_t time_now;
    char* time_string;
};

#endif
